#include "can_protocol.h"

#define EncoderUpdate(pMotor,IfAngleUpdate) \
        pMotor->EncoderSpeedUpdate((GetMotorSpeed(&(context->CANx_RxMsg.Data[0]))));\
        if(IfAngleUpdate)\
        pMotor->EncoderAngleUpdate(GetEncoderNumber(&(context->CANx_RxMsg.Data[0])));\
        pMotor->m_state_update_times ++;

static s32 GetEncoderNumber(u8* pData);
static s32 GetMotorSpeed(u8* pData);

void CAN_Decode(CANContext* context){
    if(context->CANx == CAN2){
        switch(context->CANx_RxMsg.StdId)
        {  
	

            // case (0x204 + CHASSIS_STEER_BL_MOTOR_ID):
            //     EncoderUpdate(G_sentry.chassis_steer_motor[SentryRobot::CHASSIS_S_BL_MOTOR], 1);
            //     break;      
            // case (0x204 + CHASSIS_STEER_BR_MOTOR_ID):
            //     EncoderUpdate(G_sentry.chassis_steer_motor[SentryRobot::CHASSIS_S_BR_MOTOR], 1);
            //     break
			case (0x200 + CHASSIS_LINE_BL_MOTOR_ID):
                EncoderUpdate(G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_BL_MOTOR], 0);			
                break; 
            case (0x200 + CHASSIS_LINE_BR_MOTOR_ID):
                EncoderUpdate(G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_BR_MOTOR], 0);
                break; 
            case (0x200 + CHASSIS_LINE_FR_MOTOR_ID):
                EncoderUpdate(G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_FR_MOTOR], 0);
                break; 
			case (0x200 + CHASSIS_LINE_FL_MOTOR_ID):
                EncoderUpdate(G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_FL_MOTOR], 0);
                break;	


                           
        }
    }
    else if(context->CANx == CAN1){
        switch(context->CANx_RxMsg.StdId)
        {
            // case (0x200 + CHASSIS_LINE_FL_MOTOR_ID):
            //     EncoderUpdate(G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_FL_MOTOR], 0);
            //     break;
            // case (0x200 + CHASSIS_LINE_FR_MOTOR_ID):
            //     EncoderUpdate(G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_FR_MOTOR], 0)
            //     break;
//            case (0x200 + CHASSIS_LINE_BL_MOTOR_ID):
//                EncoderUpdate(G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_BL_MOTOR], 0);
//                break; 
//				            case (0x200 + CHASSIS_LINE_FL_MOTOR_ID):
//                EncoderUpdate(G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_FL_MOTOR], 0);
//                break;		
            case (0x204 + YAW_MOTOR_ID):
                EncoderUpdate(G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR], 1);
                break;
            case 0x400:  
                G_sentry.capacitor->m_cap_vol   = (float)( ( context->CANx_RxMsg.Data[0] << 8 ) | (context->CANx_RxMsg.Data[1] ) ) / 100.0f;
                G_sentry.capacitor->m_pow_in    = (float)( ( context->CANx_RxMsg.Data[2] << 8 ) | (context->CANx_RxMsg.Data[3] ) ) / 100.0f;
                G_sentry.capacitor->m_pow_out   = (float)( ( context->CANx_RxMsg.Data[4] << 8 ) | (context->CANx_RxMsg.Data[5] ) ) / 100.0f;
                G_sentry.capacitor->m_volt_out  = (float)( ( context->CANx_RxMsg.Data[6] << 8 ) | (context->CANx_RxMsg.Data[7] ) ) / 100.0f;	
                G_sentry.capacitor->m_state_update_times++;
                break;      
        }
    }
}

/**
 *@brief Read the motor speed returned by the encoder, the available range is C610, C620, 6020 motor
 *@param pData original data array
 *@return motor speed
*/
s32 GetMotorSpeed(u8* pData)
{
	s32 speed_temp;
	s32 base_value = 0xFFFF;
	if(pData[2] & 0x01<<7 )
	{	speed_temp = (base_value<<16 | pData[2]<<8 | pData[3]);}
	else
	{	speed_temp = pData[2]<<8 | pData[3];}
	return speed_temp;
}



/**
 *@brief Read the mechanical angle returned by the encoder, the available range is C610, C620, 6020 motor
 *@param pData original data array
 *@return mechanical angle (0~8191)
*/
s32 GetEncoderNumber(u8* pData)
{
    s32 encoder_temp;
	encoder_temp = pData[0]<<8 | pData[1];
	return encoder_temp;
}
